package anyVehicle;

import java.util.HashMap;
import java.util.HashSet;
import java.util.Map;
import java.util.Set;

import javax.vecmath.Vector3f;

import net.minecraft.src.Entity;
import net.minecraft.src.EntityPlayer;
import net.minecraft.src.Vec3D;

import com.bulletphysics.collision.broadphase.CollisionFilterGroups;
import com.bulletphysics.collision.dispatch.CollisionFlags;
import com.bulletphysics.collision.dispatch.PairCachingGhostObject;
import com.bulletphysics.collision.shapes.BoxShape;
import com.bulletphysics.collision.shapes.CapsuleShape;
import com.bulletphysics.collision.shapes.CollisionShape;
import com.bulletphysics.collision.shapes.CompoundShape;
import com.bulletphysics.collision.shapes.ConvexShape;
import com.bulletphysics.dynamics.DynamicsWorld;
import com.bulletphysics.dynamics.RigidBody;
import com.bulletphysics.dynamics.RigidBodyConstructionInfo;
import com.bulletphysics.linearmath.DefaultMotionState;
import com.bulletphysics.linearmath.Transform;

public class JBulletBodyManager {

	private CollisionShape blockShape = new BoxShape(new Vector3f(0.51f, 0.51f, 0.51f));
	private Map<Vector3f, BoxShape> entityBoxShapes = new HashMap<Vector3f, BoxShape>();
	private Map<Entity, EntityKinematicController> kinematicControllers = new HashMap<Entity, EntityKinematicController>();
	private Set<EntityRigidBody> vehicleBodies = new HashSet<EntityRigidBody>();

	protected DynamicsWorld dynamicsWorld;

	private static Vector3f tmpVector = new Vector3f();

	public JBulletBodyManager(DynamicsWorld world) {
		this.dynamicsWorld = world;
	}

	public EntityRigidBody createBody(Entity entity, float mass, Transform startTransform, CollisionShape shape, boolean isKinematic) {
		Vector3f localInertia = new Vector3f(0f, 0f, 0f);
		if (isKinematic) {
			mass = 0;
		}
		if (mass > 0) {
			shape.calculateLocalInertia(mass, localInertia);
		}

		DefaultMotionState myMotionState = new DefaultMotionState(startTransform);

		RigidBodyConstructionInfo cInfo = new RigidBodyConstructionInfo(mass, myMotionState, shape, localInertia);

		EntityRigidBody body = new EntityRigidBody(entity, dynamicsWorld, cInfo);
		if (isKinematic) {
			body.setCollisionFlags(body.getCollisionFlags() | CollisionFlags.KINEMATIC_OBJECT);
			body.setActivationState(RigidBody.DISABLE_DEACTIVATION);
		}

		dynamicsWorld.addRigidBody(body);
		return body;

	}
	public EntityRigidBody createCube(Entity entity) {
		Transform t = new Transform();
		t.origin.set((float) entity.posX, (float) entity.posY, (float) entity.posZ);
		EntityRigidBody body = createBody(entity, 1f, t, blockShape, false);
		body.setUserPointer(entity);
		return body;
	}
//
//	public RigidBody createFloor() {
//		Transform t = new Transform();
//		t.origin.set(0f, )
//	}

	// Kinematic body methods

	public EntityKinematicController createEntityKinematicController(Entity entity) {
		if (entity instanceof EntityPlayer) {
			return createEntityKinematicController(entity, new CapsuleShape(0.25f, 1f));
			
		} else {
			return createEntityKinematicController(entity, getCollisionShapeFromEntity(entity));
		}
		
	}

	public EntityKinematicController createEntityKinematicController(Entity entity, ConvexShape shape) {
		Transform startTransform = new Transform();
		startTransform.setIdentity();
		startTransform.origin.set((float)entity.posX, (float)entity.posY, (float)entity.posZ);

		PairCachingGhostObject ghostObject = new PairCachingGhostObject();
		ghostObject.setWorldTransform(startTransform);

		
		ghostObject.setCollisionShape(shape);
		ghostObject.setCollisionFlags(CollisionFlags.CHARACTER_OBJECT);

		float stepHeight = 0.35f;
		EntityKinematicController character = new EntityKinematicController(entity, ghostObject, shape, stepHeight);
		character.setGravity(0f);

		dynamicsWorld.addCollisionObject(ghostObject, CollisionFilterGroups.CHARACTER_FILTER,
				(short) (CollisionFilterGroups.STATIC_FILTER | CollisionFilterGroups.DEFAULT_FILTER));

		dynamicsWorld.addAction(character);

		return character;

	}

	public EntityRigidBody createVehicleRigidBody(Vehicle vehicle) {

		CompoundShape vehicleShape = new CompoundShape();
		Transform startTransform = new Transform();
		startTransform.setIdentity();

		int numBlocks = 0;

		Vec3D corner = vehicle.getLowerBackLeftCorner();

		Vec3D corner1 = vehicle.getLowerBackLeftCorner();
		Vec3D corner2 = vehicle.getUpperFrontRightCorner(corner1);

		for (int i = (int) corner1.xCoord; i < corner2.xCoord; i++) {
			for (int j = (int) corner1.yCoord; j < corner2.yCoord; j++) {
				for (int k = (int) corner1.zCoord; k < corner2.zCoord; k++) {
					startTransform.origin.set(i, j, k);
					int block = vehicle.vehicleBlocks.getBlockId(i, j, k);
					if (block != 0) {
						vehicleShape.addChildShape(startTransform, blockShape);
						numBlocks++;
					}
				}
			}
		}

		startTransform.origin.set((float) vehicle.posX, (float) vehicle.posY, (float) vehicle.posZ);

		float mass = (float) Math.sqrt(numBlocks);
		EntityRigidBody vehicleRB = createBody(vehicle, mass, startTransform, vehicleShape, false);
		vehicleRB.setLinearVelocity(new Vector3f((float) vehicle.motionX, (float) vehicle.motionY, (float) vehicle.motionZ));
		vehicleRB.setFriction(0.5F);
		vehicleRB.setDamping(0.9F, 0.9F);
		vehicleRB.setAngularFactor(0F);
		return vehicleRB;
	}



	private ConvexShape getCollisionShapeFromEntity(Entity entity) {
		Vector3f size = VehicleUtil.getHalfSizeVector3f(entity);
		BoxShape box = entityBoxShapes.get(size);
		if (box == null) {
			box = new BoxShape(size);
			entityBoxShapes.put(size, box);
		}

		return box;
	}



	public EntityKinematicController getKinematicControllerForEntity(Entity entity) {
		EntityKinematicController controller = kinematicControllers.get(entity);
		if (controller == null) {
			controller = createEntityKinematicController(entity);
			kinematicControllers.put(entity, controller);
		}
		return controller;

	}

	public Map<Entity, EntityKinematicController> getKinematicControllers() {
		return kinematicControllers;
	}
	
	public Set<EntityRigidBody> getVehicleBodies() {
		return vehicleBodies;
	}
	
	public EntityRigidBody getVehicleBodyFromEntity(Vehicle v) {
		EntityRigidBody body = null;
		body = v.getBody();
		if (body == null) {
			body = createVehicleRigidBody(v);
			v.setBody(body);
			vehicleBodies.add(body);
		}
		return body;
	}
	
	public void removeBody(EntityRigidBody rb) {
		vehicleBodies.remove(rb);
		dynamicsWorld.removeRigidBody(rb);
	}
	
	
	
	public void removeKinematicController(EntityKinematicController c) {
		kinematicControllers.remove(c.getEntity());
		dynamicsWorld.removeAction(c);
		
	}
}
